
#include <hi_stdlib.h>
#include <hisignalling_protocol.h>
#include <hi_uart.h>

#include <app_demo_uart.h>

#include <iot_uart.h>
#include <hi_gpio.h>
#include <hi_io.h>
#include <string.h>
#include <time.h>
#include "ohos_init.h"
#include "cmsis_os2.h"

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>

#include "iot_errno.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "iot_watchdog.h"
#include "iot_pwm.h"

#define LED_INTERVAL_TIME_US 300000
#define UART_TASK_STACK_SIZE (1024 * 64)
#define UART_TASK_PRIO 25

#define IOT_Transport 9

int s3[260]={500, 500, 500, 500, 500, 511, 511, 511, 511, 522, 522, 522, 522, 533, 533, 544, 544, 544, 544, 544, 544, 544, 555, 555, 555, 
					555, 566, 566, 566, 566, 577, 577, 577, 577, 577, 588, 588, 600, 600, 600, 600, 600, 600, 600, 611, 611, 611, 611, 622, 622, 
					633, 633, 633, 633, 633, 633, 633, 644, 644, 644, 644, 655, 655, 666, 666, 666, 666, 666, 666, 677, 677, 677, 677, 677, 688, 
					688, 700, 700, 700, 700, 711, 711, 711, 711, 711, 711, 711, 722, 722, 722, 722, 733, 733, 744, 733, 744, 744, 755, 755, 755, 
					755, 755, 755, 766, 766, 766, 766, 766, 777, 788, 788, 777, 788, 788, 800, 800, 800, 800, 811, 811, 811, 811, 811, 811, 822, 
					833, 833, 822, 833, 844, 844, 844, 844, 855, 855, 855, 855, 866, 866, 866, 866, 866, 866, 877, 877, 888, 877, 888, 900, 900, 
					888, 900, 911, 911, 911, 911, 922, 911, 922, 922, 922, 933, 933, 933, 933, 944, 955, 955, 944, 955, 966, 966, 966, 977, 977, 
					977, 977, 988, 988, 988, 988, 988, 1000, 1011, 1011, 1022, 1011, 1011, 1033, 1044, 1044, 1055, 1055, 1066, 1077, 1077, 1100, 1100, 1111, 1122, 
					1122, 1144, 1144, 1155, 1166, 1188, 1188, 1200, 1211, 1211, 1244, 1244, 1266, 1277, 1288, 1288, 1311, 1333, 1333, 1366, 1366, 1377, 1422, 1433, 1444, 
					1455, 1488, 1477, 1533, 1566, 1522, 1577, 1533, 1555, 1611, 1633, 1533, 1588, 1577, 1600, 1533, 1555, 1600, 1588, 1522, 1544, 1533, 1555, 1544, 1555, 
					1544, 1533, 1522, 1588, 1577, 1533, 1544, 1555, 1533, 1533};
int s4[260]={500, 500, 511, 511, 522, 511, 511, 522, 533, 522, 522, 533, 544, 533, 533, 533, 533, 544, 544, 555, 555, 566, 555, 555, 566, 
					566, 566, 566, 577, 588, 577, 577, 588, 588, 600, 588, 588, 588, 588, 600, 600, 611, 611, 622, 611, 622, 622, 633, 622, 622, 
					622, 622, 633, 633, 644, 644, 655, 644, 655, 655, 666, 655, 655, 655, 655, 666, 666, 677, 688, 677, 677, 688, 688, 700, 688, 
					688, 688, 688, 700, 700, 700, 700, 711, 711, 722, 722, 733, 722, 733, 733, 744, 733, 733, 733, 755, 744, 744, 744, 744, 755, 
					755, 766, 766, 766, 766, 777, 777, 788, 777, 777, 777, 800, 788, 800, 788, 800, 800, 800, 800, 811, 811, 822, 822, 833, 822, 
					822, 822, 844, 833, 833, 833, 844, 844, 844, 844, 855, 855, 855, 855, 866, 866, 877, 877, 877, 877, 877, 900, 888, 888, 888, 
					911, 900, 900, 900, 911, 911, 911, 933, 922, 933, 933, 933, 933, 944, 955, 944, 944, 944, 966, 955, 955, 966, 966, 966, 966, 
					977, 977, 977, 988, 988, 1000, 1000, 1000, 1000, 1000, 1000, 1011, 1011, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 988, 988, 988, 988, 
					988, 977, 977, 977, 977, 966, 966, 966, 966, 966, 955, 955, 944, 944, 944, 944, 933, 933, 933, 922, 922, 922, 900, 900, 900, 
					900, 888, 900, 877, 866, 900, 877, 911, 911, 888, 888, 955, 933, 955, 955, 1000, 1000, 988, 1011, 1055, 1055, 1077, 1077, 1100, 1111, 
					1133, 1155, 1177, 1166, 1188, 1233, 1255, 1266, 1311, 1344};
int s5[260]={1355, 1355, 1366, 1366, 1377, 1366, 1366, 1377, 1388, 1388, 1388, 1400, 1411, 1400, 1400, 1411, 1411, 1422, 1422, 1433, 1433, 1444, 1433, 1433, 1444, 
					1444, 1455, 1455, 1466, 1477, 1466, 1466, 1477, 1477, 1488, 1477, 1477, 1488, 1488, 1500, 1500, 1511, 1511, 1522, 1511, 1522, 1522, 1533, 1522, 1522, 
					1533, 1533, 1544, 1544, 1555, 1555, 1566, 1555, 1566, 1566, 1577, 1566, 1566, 1577, 1577, 1588, 1588, 1600, 1611, 1600, 1600, 1611, 1611, 1622, 1611, 
					1611, 1622, 1622, 1633, 1633, 1633, 1633, 1644, 1644, 1655, 1655, 1666, 1655, 1666, 1666, 1677, 1666, 1666, 1677, 1688, 1688, 1688, 1688, 1688, 1700, 
					1700, 1711, 1711, 1711, 1711, 1722, 1722, 1733, 1722, 1733, 1733, 1744, 1733, 1744, 1744, 1755, 1755, 1744, 1755, 1766, 1766, 1777, 1777, 1777, 1777, 
					1788, 1788, 1800, 1788, 1800, 1800, 1811, 1811, 1811, 1811, 1822, 1822, 1822, 1822, 1833, 1833, 1844, 1844, 1844, 1844, 1855, 1866, 1855, 1866, 1866, 
					1877, 1866, 1877, 1877, 1888, 1888, 1888, 1900, 1900, 1911, 1911, 1911, 1911, 1922, 1933, 1922, 1933, 1933, 1944, 1933, 1944, 1955, 1955, 1955, 1955, 
					1966, 1966, 1966, 1977, 1977, 1988, 1988, 1988, 2000, 2000, 2000, 1988, 1988, 2000, 2000, 2000, 2000, 2000, 2011, 2011, 2011, 2011, 2011, 2022, 2022, 
					2022, 2022, 2022, 2033, 2033, 2033, 2033, 2044, 2044, 2044, 2055, 2055, 2055, 2066, 2066, 2066, 2066, 2077, 2077, 2088, 2088, 2088, 2100, 2100, 2111, 
					2111, 2122, 2122, 2133, 2144, 2144, 2155, 2155, 2166, 2177, 2188, 2188, 2200, 2211, 2222, 2222, 2233, 2244, 2255, 2255, 2266, 2277, 2288, 2300, 2311, 
					2322, 2333, 2344, 2366, 2377, 2388, 2411, 2422, 2444, 2466};



static const char *data = "Start work\r\n";
static const char *data1 = "Start work1\r\n";
UartDefConfig uartDefConfig = {0};

static void Uart12GpioCOnfig(void)
{
#ifdef ROBOT_BOARD
    IoSetFunc(HI_IO_NAME_GPIO_5, IOT_IO_FUNC_GPIO_5_UART1_RXD);
    IoSetFunc(HI_IO_NAME_GPIO_6, IOT_IO_FUNC_GPIO_6_UART1_TXD);
    /* IOT_BOARD */
#elif defined (EXPANSION_BOARD)
    IoSetFunc(HI_IO_NAME_GPIO_0, IOT_IO_FUNC_GPIO_0_UART1_TXD);
    IoSetFunc(HI_IO_NAME_GPIO_1, IOT_IO_FUNC_GPIO_1_UART1_RXD);
    IoSetFunc(HI_IO_NAME_GPIO_11, IOT_IO_FUNC_GPIO_11_UART2_TXD);
    IoSetFunc(HI_IO_NAME_GPIO_12, IOT_IO_FUNC_GPIO_12_UART2_RXD);
    IoTGpioInit(IOT_Transport);
    IoSetFunc(IOT_Transport, 0);
    IoTGpioSetDir(IOT_Transport, IOT_GPIO_DIR_OUT);

#endif
}


int SetUartRecvFlag(UartRecvDef def)
{
    if (def == UART_RECV_TRUE) {
        uartDefConfig.g_uartReceiveFlag = HI_TRUE;
    } else {
        uartDefConfig.g_uartReceiveFlag = HI_FALSE;
    }
    
    return uartDefConfig.g_uartReceiveFlag;
}

int GetUartConfig(UartDefType type)
{
    int receive = 0;

    switch (type) {
        case UART_RECEIVE_FLAG:
            receive = uartDefConfig.g_uartReceiveFlag;
            break;
        case UART_RECVIVE_LEN:
            receive = uartDefConfig.g_uartLen;
            break;
        default:
            break;
    }
    return receive;
}

void ResetUartReceiveMsg(void)
{
    (void)memset_s(uartDefConfig.g_receiveUartBuff, sizeof(uartDefConfig.g_receiveUartBuff),
        0x0, sizeof(uartDefConfig.g_receiveUartBuff));
}

unsigned char *GetUartReceiveMsg(void)
{
    return uartDefConfig.g_receiveUartBuff;
}

//int counter1=0;
int counter = 0;
int reset_flag = 1;
//int counter2=0;

static hi_void *UartDemoTask(char *param)
{
    hi_unref_param(param);
    hi_u8 uartBuff1[UART_BUFF_SIZE] = {0};
    uint8_t *uart_buff_ptr1 = uartBuff1;

    int mx=0,mx1=0,mx2=0,mx3=0,mx4=0;
    int my=0,my1=0,my2=0,my3=0,my4=0;
    int classes=0;

    IotUartAttribute uartAttr1 = {
        .baudRate = 115200, 
        .dataBits = 8, 
        .stopBits = 1, 
        .parity = 0,
    };
    IotUartAttribute uartAttr2 = {
        .baudRate = 9600, 
        .dataBits = 8, 
        .stopBits = 1, 
        .parity = 0,
    };
    IoTUartInit(HI_UART_IDX_2, &uartAttr2);
	IoTGpioSetOutputVal(IOT_Transport, 1);
	
    for (;;) {         
		IoTUartInit(HI_UART_IDX_1, &uartAttr1);
		TaskMsleep(50);
        IoTUartRead(HI_UART_IDX_1, uartBuff1, UART_BUFF_SIZE);
        if((uartBuff1[0]==0xaa)&&(uartBuff1[1]=0x55)){
			IoTUartDeinit(HI_UART_IDX_1);
			TaskMsleep(50);
			//counter = 0;

            classes=uartBuff1[2]-0x30;
            mx1=uartBuff1[3]-0x30;
            mx2=uartBuff1[4]-0x30;
            mx3=uartBuff1[5]-0x30;
            mx4=uartBuff1[6]-0x30;
            my1=uartBuff1[7]-0x30;
            my2=uartBuff1[8]-0x30;
            my3=uartBuff1[9]-0x30;
            my4=uartBuff1[10]-0x30;
            mx=1920-(mx1*1000+mx2*100+mx3*10+mx4);
            my=my1*1000+my2*100+my3*10+my4;
            if (classes >= 1 && classes <= 8) servos(mx,my,classes); 
                            
        }
		memset(uartBuff1,0,sizeof(uartBuff1));
		counter++;
		if(counter>=30){
			//counter1=0;
			//counter2=0;
			counter=0;
			if (reset_flag) reset();
			reset_flag = 0;
		}
        }

    return HI_NULL;
} 
void reset()
{
	char order_char[25]={0};
	int order[22] = {0x55, 0x55, 0x14, 0x03, 0x05, 0x08, 0x03, 0x01, 0xdc, 0x05, 0x03, 0xf4, 0x01, 0x04, 0xf4, 0x01, 0x05, 0x6d, 0x03, 0x06, 0xdc, 0x05};
	for (int i=0;i<=21;i++)
		order_char[i]=order[i]+'0'-48;
	order_char[22]=13+'0'-48;
	order_char[23]=10+'0'-48;
	IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char, strlen(order_char));
	TaskMsleep(1200);
}

void servos(int x0, int y0, int kind) 
{
	int order[88] = {0x55, 0x55, 0x14, 0x03, 0x05, 0xe8, 0x03, 0x01, 0xf4, 0x01, 0x03, 0xff, 0xff, 0x04, 0xff, 0xff, 0x05, 0xff, 0xff, 0x06, 0xff, 0xff,
					  0x55, 0x55, 0x08, 0x03, 0x01, 0xf4, 0x01, 0x01, 0xc4, 0x09,
					  0x55, 0x55, 0x08, 0x03, 0x01, 0xf4, 0x01, 0x05, 0xff, 0xff,
					  0x55, 0x55, 0x0e, 0x03, 0x03, 0xf4, 0x01, 0x03, 0xff, 0xff, 0x04, 0xff, 0xff, 0x05, 0xff, 0xff,
					  0x55, 0x55, 0x08, 0x03, 0x01, 0xee, 0x02, 0x06, 0xff, 0xff,
					  0x55, 0x55, 0x08, 0x03, 0x01, 0xf4, 0x01, 0x01, 0xf4, 0x01,
					  //0x55, 0x55, 0x14, 0x03, 0x05, 0x08, 0x03, 0x01, 0xdc, 0x05, 0x03, 0xb8, 0x05, 0x04, 0x62, 0x04, 0x05, 0x57, 0x04, 0x06, 0xdc, 0x05
					  0x55, 0x55, 0x08, 0x03, 0x01, 0xf4, 0x01, 0x06, 0xff, 0xff};
	char order_char1[25]={0};
    char order_char2[13]={0};
    char order_char3[13]={0};
    char order_char4[19]={0};
    char order_char5[13]={0};
    char order_char6[13]={0};
    char order_char7[13]={0};


    float x, y;      
	int dis;          
	float theta;      
	int servo3, servo4, servo5, servo6;
	int servo3_put, servo4_put, servo5_put, servo6_put;
	int servo6_put2;
	//x0 = 800; y0 = 700; kind = 4;
	
	//x = 0.382 * x0 - 418.14;
	x = 219 * (x0 - 960) / 640;
	y = 337 - 44 * y0 / 135;
	//y = -0.465 * y0 + 428.75;
	//x = 0;
	//y=177;
	dis = sqrt(x*x+y*y);
	if (y==0)
	{
		if (x<0) servo6 = 500;
		else servo6 = 2500;
	}
	else
	{
		theta = atan(x/y);
		servo6 = 2000 * theta / 3.14159 + 1500;
	}
	//if (dis < 100) dis = 100;
	//if (dis > 359) dis = 359;
	if (dis >= 120 && dis <= 315 && servo6 >= 1080 && servo6 <= 1920 && kind >= 1 && kind <= 8) 
	{
		counter = 0;
		reset_flag = 1;
		//printf("ok\n");
		servo3 = s3[dis - 100]; 
		servo4 = s4[dis - 100]; 
		servo5 = s5[dis - 100]; 
		order[11] = servo3 % 256; order[12] = servo3 / 256;
		order[14] = servo4 % 256; order[15] = servo4 / 256;
		order[17] = servo5 % 256; order[18] = servo5 / 256;
		order[20] = servo6 % 256; order[21] = servo6 / 256;
	
		servo5 -= 200;
		if (servo5 < 500) servo5 = 500;
		order[40] = servo5 % 256; order[41] = servo5 / 256;
	
		int dis_put=rand()%130+160;
		servo3_put = s3[dis_put - 100];
		servo4_put = s4[dis_put - 100];
		servo5_put = s5[dis_put - 100] - 300;
		if (kind >= 1 && kind <= 3)
		{
			//servo3_put = 1104; servo4_put = 1033; servo5_put = 1698; 
			servo6_put = 510;
			servo6_put2 = 1000;
		}
		else if (kind >= 4 && kind <= 6)
		{
			//servo3_put = 1392; servo4_put = 1356; servo5_put = 2111; 
			servo6_put = 813;
			servo6_put2 = 1000;
		}
		else if (kind == 7)
		{
			//servo3_put = 1392; servo4_put = 1356; servo5_put = 2111; 
			servo6_put = 2187;
			servo6_put2 = 2000;
		}
		else if (kind == 8)
		{
			//servo3_put = 1104; servo4_put = 1033; servo5_put = 1698; 
			servo6_put = 2490;
			servo6_put2 = 2000;
		}
		order[50] = servo3_put % 256; order[51] = servo3_put / 256;
		order[53] = servo4_put % 256; order[54] = servo4_put / 256;
		order[56] = servo5_put % 256; order[57] = servo5_put / 256;
		order[66] = servo6_put % 256; order[67] = servo6_put / 256;
		order[86] = servo6_put2 % 256; order[87] = servo6_put2 / 256;
	
		for (int i=0;i<=21;i++)
        	order_char1[i]=order[i]+'0'-48;
		//if(x0>640&&x0<1280){
		//	order_char1[8]=0xf4+'0'-48;
		//	order_char1[9]='0'-48+0x01;
		//}
		//order_char1[22]=13+'0'-48;
		order_char1[22] = kind;
		order_char1[23]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char1, strlen(order_char1));
	    TaskMsleep(1200);
	
		for (int i=22;i<=31;i++)
        	order_char2[i-22]=order[i]+'0'-48;
		//order_char2[10]=13+'0'-48;
		order_char2[10] = reset_flag;
		order_char2[11]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char2, strlen(order_char2));
	    TaskMsleep(600);
    
		for (int i=32;i<=41;i++)
        	order_char3[i-32]=order[i]+'0'-48;
		order_char3[10]=13+'0'-48;
		order_char3[11]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char3, strlen(order_char3));
	    TaskMsleep(600);
	
		for (int i=42;i<=57;i++)
        	order_char4[i-42]=order[i]+'0'-48;
		order_char4[16]=13+'0'-48;
		order_char4[17]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char4, strlen(order_char4));
	    TaskMsleep(600);
	
		for (int i=58;i<=67;i++)
        	order_char5[i-58]=order[i]+'0'-48;
		order_char5[10]=13+'0'-48;
		order_char5[11]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char5, strlen(order_char5));
	    TaskMsleep(1000);
	
		for (int i=68;i<=77;i++)
    		order_char6[i-68]=order[i]+'0'-48;
		order_char6[10]=13+'0'-48;
		order_char6[11]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char6, strlen(order_char6));
	    TaskMsleep(600);

		for (int i=78;i<=87;i++)
        	order_char7[i-78]=order[i]+'0'-48;
		order_char7[10]=13+'0'-48;
		order_char7[11]=10+'0'-48;
		IoTUartWrite(HI_UART_IDX_2, (unsigned char *)order_char7, strlen(order_char7));
	    TaskMsleep(600);
	}
	
}
hi_void UartTransmit(hi_void)
{   
    Uart12GpioCOnfig();
    osThreadAttr_t attr;

    attr.name = "UartTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = UART_TASK_STACK_SIZE;
    attr.priority = UART_TASK_PRIO;

    if (osThreadNew((osThreadFunc_t)UartDemoTask, NULL, &attr) == NULL) {
        printf("Failed to create UartTask!\n");
    }
}
SYS_RUN(UartTransmit);